#pragma config(Sensor, in1,    liftHeightPot,       sensorPotentiometer)
#pragma config(Sensor, dgtl1,  leftEncoder,         sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  rightEncoder,        sensorQuadEncoder)
#pragma config(Sensor, dgtl6,  transmissionLimitSwitch, sensorTouch)
#pragma config(Sensor, dgtl7,  liftEncoder,         sensorQuadEncoder)
#pragma config(Sensor, dgtl9,  liftHeightPot,       sensorRotation)
#pragma config(Sensor, dgtl12, autoColor,           sensorDigitalIn)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//




#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!


int armAverage();
void liftArm(int liftHeight);
void armSampleUpdate ();


#ifndef VEX_FUNCTIONS_H
#include "Vex_Functions.h"
#endif

#ifndef VEX_FUNCTIONS_C
#include "Vex_Functions.c"
#endif





//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*=======================ASSIGNMENTS==========================
======MOTORS=====
port1=back left drive
port2=middle left drive
port3=front right drive
port4=middle right drive
port5=hooks
port6=left lift motor
port7=transmission motor
port8=grabber motor
port9=front left drive
port10=back right drive
=====SENSORS=====
=Analog=
in1=liftHeightPot
=Digital=
dgtl 1 and 2=leftEncoder
dgtl 3 and 4=rightEncoder
dgtl 6=transmissionLimitSwitch
dgtl 12= Jumper in = blue, jumper out = red

============STATES=================
state 0=runs regular drive code
state 1=transmission in and lift up
state 2=run winch full speed
state 3=winch driver control
*/

//============================PUT GLOBAL VARIABLES HERE=========================

//This is for the lift height to make it at lowest position.
int liftHeight = 0;
//This is to slow down motors as you approach scoring height.
float motorSlowDownDst = 100.0;
//This is to run motor a little more to account for gravity.
int gravityCounter = 0;
//Holds x number of values for arm average
#define NAS 10
int armSampleSlots = NAS;
int armSamples[NAS];
// Arm heights
int lowArmHeight = 560;
int medArmHeight = 560;
int highArmHeight = 560;
int winchArmHeight = 1066;


//This is to move gears on winch into place.
int transmissionIn = -22;
//This is to move gears out
int transmissionOut = 127;
//
int INGEAR = 1;
//This is to run drive motors so winch gears mesh with drive gears.
int transmissionDriveMotor = 50;
bool shiftforward = true;
int shiftcounter = 0;
//This is to open and close grabber. 0=closed 1=open
int grabberServoOpen = -100;
int grabberServoClose = 100;
bool grabberPosition = 0;
//debouncer for grabber
bool debouncer = false;
//Preset Arm Heights
bool debouncer8R = false;

//For shifting and winch to be able to get out
int state = 0;


//reads potentiometer armAverage
int temp;

//represent number being sent to motors
int leftLimited;
int rightLimited;

//just counts numbers of times through the code for limiting function.
int counter = 0;

//Temporary....

int averageheight;


#ifndef AUTONOMOUS_C
#include "Autonomous.c"
#endif




//==========================================FUNCTIONS======================================================================================
//=========================================================================================================================================
/************************************
*Purpose/Description: Drive motors but not accelerate quickly
************************************/

void drive(int leftDesired, int rightDesired)

  {
   motor[port1] = motor[port2]= motor[port9] = leftDesired;
   motor[port3] = motor[port4] =motor[port10] = rightDesired;
  }






/************************************
*Purpose/Description: Slots to make an average for arm height.
************************************/
void armSampleUpdate ()
{
  for (int j=armSampleSlots-1; j>0; j--)
  {
    armSamples[j]= armSamples[j-1];
    //armSamples[5]= 20;
  }
  armSamples[0]=SensorValue[liftEncoder];
}


/************************************
*Purpose/Description: Finding average for arm height.
************************************/
int armAverage()
{
  long runningTotal = 0;
  for (int k=0; k<armSampleSlots; k++)
  {
    runningTotal = runningTotal + armSamples[k];
  }
  return runningTotal/armSampleSlots;
}


/************************************
*Purpose/Description:Drive lift motors to desired lift height
************************************/
void liftArm(int liftHeight)
{
  int armave = armAverage();
   // Runs motor full speed until it reaches close to wanted height.
   if ( armAverage() < liftHeight-motorSlowDownDst+(gravityCounter*motorSlowDownDst/127))
   {
      motor[port6] = 127;
   }
   //// Runs motor backwards full speed until it reaches close to wanted height.
   else if (armAverage() > liftHeight+motorSlowDownDst+(gravityCounter*motorSlowDownDst/127))
   {
     motor[port6] = -127;
   }
   //As arm approaches exact lift height, it slows down linearly.
   else
   {
	   //TODO: possible casting issues between float and int
     motor[port6]=(-127.0/motorSlowDownDst)*(armAverage()-liftHeight)+ gravityCounter;
   }
}//End liftArm


/************************************
*Purpose/Description:This moves motors so the gears mesh easier.
************************************/
void motorshake()
{
        shiftcounter ++;

        if (shiftcounter > 2000)
        {
          shiftforward = !shiftforward;
          shiftcounter = 0;
        }

        if(shiftforward)
        {


        motor[port1]= transmissionDriveMotor;
        motor[port2]= transmissionDriveMotor;
        motor[port3]= transmissionDriveMotor;
        motor[port4]= transmissionDriveMotor;
        motor[port9]= transmissionDriveMotor;
        motor[port10]=transmissionDriveMotor;
        }
        else
        {
        motor[port1]= -transmissionDriveMotor;
        motor[port2]= -transmissionDriveMotor;
        motor[port3]= -transmissionDriveMotor;
        motor[port4]= -transmissionDriveMotor;
        motor[port9]= -transmissionDriveMotor;
        motor[port10]=-transmissionDriveMotor;
        }
}// end motorshake


/************************************
*Purpose/Description: Limits joystick sensitivity
************************************/
int deadband(int motorpower)
{
  if (motorpower > -17 && motorpower < 17)
  {
    motorpower = 0;
  }
  return motorpower;
}

/************************************
*Purpose/Description: Function to calculate encoder value from potentiometer value.
************************************/
int encoderCalc(int liftPot)
{

return ((6*liftPot)-(6*lowArmHeight))/131;

}

/************************************
*Purpose/Description: Lift code
************************************/
void armcontrol()
{
	   //Set desired Lift height
	   if (vexRT[Btn6DXmtr2] == 1)
	   {
	      liftArm(encoderCalc(lowArmHeight));
	   }
	   else if (vexRT[Btn5DXmtr2] == 1)
	   {
	      liftArm(encoderCalc(medArmHeight));
	   }
	   else if (vexRT[Btn5UXmtr2] == 1)
	   {
	      liftArm(encoderCalc(highArmHeight));
	   }
	   //allows driver control of arm by position control.
	   else
	   {
	     motor[port6]=limitMotorPower(vexRT[Ch3Xmtr2]+gravityCounter);
    	//TODO: Adjust limit arm motion     liftArm(((highArmHeight-lowArmHeight) * vexRT[Ch2Xmtr2]/254)+ (highArmHeight+lowArmHeight)/2);
	   }
	   //liftArm(liftHeight)*/


	   //===========================Hooks for Ladder=================================

     motor[port5]=deadband(vexRT[Ch2Xmtr2]);
}//end armcontrol


/************************************
*Purpose/Description: Detects what stage to be in.
************************************/
void detectStateChanges()
{
   if(vexRT[Btn8R]==1 && state<2 && debouncer8R == false)
   {
     debouncer8R = true;
     state++;
   }
   if(vexRT[Btn8L]==1 && state!=2)
   {
     state=0;
   }
   if(vexRT[Btn8U]==1 && state == 2)
   {
     state=3;
   }
   if(vexRT[Btn8R]==0)
   {
     debouncer8R = false;
   }

}


/************************************
*Purpose/Description: Resets encoder to 0 before start.
************************************/
void pre_auton()
{
	// All activities that occur before the competition starts
	// Example: clearing encoders, setting servo positions, ...
  SensorValue[liftEncoder] = 0;
}


//==============================================================================================================================================
//==============================================================================================================================================



task autonomous()
{
  // .....................................................................................
  // Insert user code here.
  // .....................................................................................
   armSampleUpdate();
   godzilla(true);
	//auto(SensorValue[autoColor],false);

}





task usercontrol()
{
  // bIfiAutonomousMode = false;
  bMotorReflected[port4]=1;
  bMotorReflected[port9]=1;
  bMotorReflected[port10]=1;
  motor[port7]=transmissionOut;
  wait1Msec(500);


  //======================TESTING LOOP==========================
  /*while(1){

     motor[port1] = motor[port2]= motor[port9] = VexRT[Ch3];
     motor[port3] = motor[port4] =motor[port10]= VexRT[Ch2];
  }*/

  //==========================Autonomous Blue Right=============================




 while(true)
 {


    //Read values for Arm Sample Upadate array
    armSampleUpdate();
    armcontrol();
    detectStateChanges();
    //=============STATE 0 (runs regular drive code)==================================
    if (state ==0)
    {
	     while(SensorValue[transmissionLimitSwitch]==INGEAR)
	     {
	        motor[port7]=transmissionOut;
	        motorshake();
	     }
		   //=====================DRIVE CODE==========================
		   if (vexRT[Btn5U] == 1)
		   {
		      //Port 1 and 2=left motors
		     drive(deadband(vexRT[Ch3]/2),deadband(vexRT[Ch2]/2));
		   }
		   else
		   {
		     //Port 1 and 2=left motors
		      drive(deadband(vexRT[Ch3]),deadband(vexRT[Ch2]));
		   }
		   //===================GRABBER CODE======================
		   //Grabber Position toggling
		   if (vexRT[Btn8DXmtr2] == 1 && debouncer == false)
		   {
		      grabberPosition = !grabberPosition;
		      debouncer = true;
		   }
		   else if (vexRT[Btn8DXmtr2] == 0)
		   {
		      debouncer = false;
		   }
		   // drive grabber motors
		   if (grabberPosition)
		   {
		      motor[port8]=grabberServoOpen;
		   }
		   else
		   {
		      motor[port8]=grabberServoClose;
		   }

  }//end state 0

  // =============STATE 1 (transmission in and lift up)==================================
  if(state==1)
  {
     //Shift into gear
     while(SensorValue[transmissionLimitSwitch] != INGEAR)
     {
        motor[port7]= transmissionIn;

        motorshake();
     }
     //liftArm(winchArmHeight);
     motor[port1]= motor[port2]= motor[port3]= motor[port4]= motor[port9]= motor[port10]=0;
  }//end state 1


  //TODO: Take out state 2...it is unneeded...
  // =============STATE 2 (run winch full speed)==================================
  if(state== 2)
  {
     //Pull up the ladder
     // Once transmission shifts in, press button again to run the winch up.
     motor[port1]= 127;
     motor[port2]= 127;
     motor[port3]= 127;
     motor[port4]= 127;
     motor[port9]= 127;
     motor[port10]= 127;
     //liftArm(medArmHeight);
  }
  // =============STATE 3 (winch driver control)==================================
  if(state==3)
  {
     motor[port1] = motor[port2] = motor[port9] =deadband(vexRT[Ch2]);
	     //Port 3 and 4=right motors
	   motor[port3] = motor[port4] =motor[port10] =deadband(vexRT[Ch2]);
	}

  }// end while(1)
}// End usercontrol
